#include "stdio.h"
#include "stdlib.h"
#include "inc/core/tridiagonal.h"

void do_tridiagonal_solve(const double *a, const double *b, double *c, double *d, double *x, int n) {

    int i = 0;
	c[0] /= b[0];	/* Division by zero risk. */
	d[0] /= b[0];
    for ( i = 1; i < n; i++) {
		
        floatdig id = 1 / (b[i] - c[i - 1] * a[i]); /* Division by zero risk. */
#ifdef DEBUG
		if (b[i] - c[i - 1] * a[i] == 0)
			fprintf(stdout, "!ERROR: do_triagonal_solve - division by zero!\n");
#endif
        c[i] *= id; /* Last value calculated is redundant. */
        d[i] = (d[i] - d[i - 1] * a[i]) * id;
    }

    /* Now back substitute. */
    x[n - 1] = d[n - 1];
    for (i = n - 2; i >= 0; i--)
        x[i] = d[i] - c[i] * x[i + 1];
}

void tridiagonal_init_matrix(int size, tridiagonal_matrix* matrix) {
    int i = 0;
	floatdig* a;
    floatdig* b;
    floatdig* c;
	int msize = size;
	matrix->size = msize;
    matrix->a = (floatdig*) malloc(sizeof (floatdig) * msize);
    matrix->b = (floatdig*) malloc(sizeof (floatdig) * msize);
    matrix->c = (floatdig*) malloc(sizeof (floatdig) * msize);
	a = matrix->a;
	b = matrix->b;
	c = matrix->c;

    //set the matrix for soloving Neimahn's problem
	for (i = 0; i < msize; i++) {
        a[i] = 0;
        b[i] = 0;
        c[i] = 0;
    }

    b[0] = 1;
    b[msize - 1] = 1;
    
    for (i = 0; i < msize-1; i++) {
        a[i] = 1;
    }
    for (i = 1; i < msize; i++) {
        c[i] = 1;
    }
    for (i = 1; i < msize - 1; i++) {
        b[i] = -2;
    }
}

void tridiagonal_clear_matrix(tridiagonal_matrix* matrix) {
    free(matrix->a);
    free(matrix->b);
    free(matrix->c);
}

void tridiagonal_solve(tridiagonal_matrix* matrix, floatdig* lhand, floatdig* x) {
    do_tridiagonal_solve(matrix->a, matrix->b, matrix->c, lhand, x, matrix->size);
}